#include "Distance.h"

namespace common {

Distance::Distance(int id,Point* k,Point* l,Parameter* param):common::Constraint(id,1,DISTANCE){
	setK(k);
	setL(l);
	setM(NULL);
	setN(NULL);
	setParameter(param);
}
ublasVector Distance::getValue() throw(MyException){
	ublasVector out(1);
	out(0) = ((*getL()-*getK()).length() - getParameter()->getValue());
	return out;
}
ublasMatrix Distance::getJacobianOnPoint(Point* p) throw(MyException){
	ublasMatrix mout(1,2);

	Vector lkC((*getL()-*getK()).unit());
	ublasVector lk(2);
	lk << lkC;

	if(k==p){
		row(mout,0)= -lk;
	}
	else if(l==p){
		row(mout,0)= lk;
	}

	return mout;
}
Distance::~Distance() {

}

}
